#include <iostream>
#include "graph.h"
#include "map.h"
#include "los.h"

Graph::Graph(void)
{
}

Graph::~Graph(void) 
{
}

void Graph::addNode(const NodeIndex& index)
{
  if (mNodes.find(index) == mNodes.end())
    mNodes.insert(std::make_pair(index, new Node(index)));
}

void Graph::removeNode(Node* node)
{
//   // Remove this node from all its neighbors
//   NodeMapItr itr = node->getIterator();
//   while (itr.hasMoreElements())
//   {
//     Node* neighbor = itr.getNext();
//     if (neighbor != node) neighbor->removeNeighbor(node);
//   }
// 
//   // Remove this node from the map
//   mNodes.erase(mNodes.find(node->getIndex()));
}

void Graph::calculateEdges(const Map& map)
{
  // Clear all existing edges
  NodeMap::iterator itr1 = mNodes.begin();
  for (; itr1 != mNodes.end(); ++itr1)
    itr1->second->clearNeighbors();

  // Find all edges by figuring out which nodes can "see" which other nodes.
  for (itr1 = mNodes.begin(); itr1 != mNodes.end(); ++itr1)
  {
    Node* node1 = itr1->second;

    NodeMap::iterator itr2 = itr1;
    ++itr2;
    for (; itr2 != mNodes.end(); ++itr2)
    {
      Node* node2 = itr2->second;

      // Ignore the same index (should never happen, but just to be safe)
      if (node1->getIndex() == node2->getIndex()) continue;

      // See if these two nodes have line of sight to each other
      if (isInLos(map, node1->getIndex(), node2->getIndex()))
      {
        // If so, create an edge between these two
        node1->addNeighbor(node2);
        node2->addNeighbor(node1);
      }
    }
  }
}

void Graph::clearNodes(void)
{
  NodeMap::iterator itr = mNodes.begin();
  for (; itr != mNodes.end(); ++itr)
    delete itr->second;
  mNodes.clear();
}

Node* Graph::operator [] (const NodeIndex& index)
{
  NodeMap::iterator itr = mNodes.find(index);
  if (itr != mNodes.end())
    return itr->second;
  return NULL;
}

const NodeMap& Graph::getNodes(void) const
{
  return mNodes;
}
